package oldFiles;
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;

public class WallFollower {
  private final static int SAMPLE_FREQUENCE = 15; //original 300
  private final static float PERCENTAGE_POWER = 0.75f;	
  
  public static void main (String[] aArg)
  throws Exception
  {
     UltrasonicSensor us = new UltrasonicSensor(SensorPort.S1);
     
     final int  noObject = 255;
     int distance,
         desiredDistance = 35, // original 35 cm
         power,  
         minPower = 60; // original 60
     
     int leftPower, rightPower;
     
     float error, gain = 0.5f; //original 0.5f
	  
     LCD.drawString("Distance: ", 0, 1);
     LCD.drawString("LPwr:     ", 0, 2);
     LCD.drawString("RPwr:     ", 0, 3);
	   
     while (! Button.ESCAPE.isPressed())
     {		   
         distance = us.getDistance();
		 
         if ( distance != noObject ) 
         {
             error = distance - desiredDistance;
             power = (int)(gain * error);
             if ( error > 0 )
             { 
                 //power = Math.min(minPower + power,100);
                 //Car.forward(power,power);
                 //LCD.drawString("Forward ", 0, 3);
            	 leftPower = Math.min(minPower + Math.abs(power),100);
            	 rightPower = (int)(leftPower * PERCENTAGE_POWER);
            	 //Car.forward(leftPower, rightPower);
            	 LCD.drawString("*Turning left ", 0, 4);
             }
             else 
             {
                 //power = Math.min(minPower + Math.abs(power),100);
                 //Car.backward(power, power);
		         //LCD.drawString("Backward", 0, 3);
            	 rightPower = Math.min(minPower + Math.abs(power),100);
            	 leftPower = (int)(rightPower * PERCENTAGE_POWER);
            	// Car.forward(leftPower, rightPower);
            	 LCD.drawString("*Turning right ", 0, 4);
		    	 
             }
             LCD.drawInt(distance,4,10,1);
             LCD.drawInt(leftPower, 4,10,2);
             LCD.drawInt(rightPower, 4,10,3);
             
		 }
         else {
             leftPower = 100;
             rightPower = 100;
             LCD.drawString("Drive straight ", 0, 4);
             
         }
         Car.forward(leftPower, rightPower);
         
         LCD.drawInt(distance,4,10,1);
         LCD.drawInt(leftPower, 4,10,2);
         LCD.drawInt(rightPower, 4,10,3);
		 
         Thread.sleep(SAMPLE_FREQUENCE);
     }

  }
}